
import struct 
import math 
import time 

from Brain.io.core import Comm
from operations import Operation, PREDEFINED_POSITIONS, GLOBAL_OPERATIONS

class MoveHistoryElement:
	def __init__(self, joint_number, angle, speed, time_stamp):
		self.joint_number = joint_number
		self.angle = angle
		self.speed = speed
		self.time_stamp = time_stamp

class Arm:
	""" Robotic arm control 
		exposes method of capturing arm positions,
		collection of positions which can be selected
	"""
	MIN_ANGLE = 150
	MAX_ANGLE = 1500
	MAX_Z = 13.0
	
	def __init__(self):
		self._joints = [778, 833, 602, 536, 750, 496, ]
		self._recorded_steps = []
		self.pipe = Comm.link('arm')
		self.move_to_position('start', 12)
		
		## number of caried items
		self._load = 0
		
		self.operations = GLOBAL_OPERATIONS.copy()
	
	def record_reset(self):
		""" reset track recording
		"""
		self._recorded_steps = []
	
	def get_history(self):
		return self._recorded_steps[:]
	
	def recording_add_step(self, joint_number, angle, speed):
		now = time.time()
		self._recorded_steps.append( MoveHistoryElement(joint_number, angle, speed, now) )
	
	def write(self, joint_number, angle, speed):
		""" LOW LEVEL INTERFACE
			change joint angle
		"""
		data = '!SC' 
		packet = struct.pack('<BBHB', int(joint_number), int(speed), int(angle), int(13))
		data += packet
		self.pipe.write(data)
		self.recording_add_step( joint_number, angle, speed )

	def move_incremental(self, joint_number, angle, speed):
		""" LOW LEVEL INTERFACE
			change joint angle
		"""
		value = self._joints[joint_number]
		value += angle
		if self.MIN_ANGLE < value < self.MAX_ANGLE:
			self._joints[joint_number] = value
			self.write(joint_number, value, speed)
	
	def move_to_position(self, name, speed):
		if name not in PREDEFINED_POSITIONS:
			print '[arm] undefined position %s' % (name, )
			return False
		joints_information = PREDEFINED_POSITIONS[name][:]
		if len(joints_information) != 6:
			print '[arm] %s position has %s joints !' % (name, len(joints_information), )
			return False
		joint_index = 0
		
		self.set_raw_position(joints_information)
	
	def set_raw_position(self, information, speeds = [12, 12, 12, 12, 12, 12,]):
		joint_index = 0
		for angle in information:
			if angle:
				self.write(joint_index, angle, speeds[joint_index]) 
				self._joints[joint_index] = angle 
			joint_index += 1

	def move(self, x, y, z, speeds = [12, 12, 12, 12, 12, 12,]):
		""" HIGH LEVEL INTERFACE
			uses inverse kinematics calculations
		"""
		joints_information = self.calculate_joints(x, y, z)
		## send to arm:
		joint_index = 0
		self.set_raw_position(joints_information, speeds)
	
	def _move_smootly(self, begin_x, begin_y, x, y, z):
		"""
		"""
		X_korak, Y_korak, ox, oy, raz, raz_korak, = 2.0, 2.0, 0.0, begin_y, 0.0, 0.0 ,
		oy, ox, y, x, = y, x + begin_x, 0.0, begin_x

		if abs(oy) == 0:
			raz=1000
		else:
			if abs(ox) > abs(oy):
				raz = abs(ox/oy)
				raz_korak = abs(ox/oy)
			if abs(oy) > abs(ox):
				raz = abs(oy/ox)
				raz_korak = abs(oy/ox)

		if(oy<0): Y_korak = -1
		
		while 1:
			#if abs(ox - x) < self.TOLERANCE and abs(oy - y) < self.TOLERANCE: break
			
			x_, y_, z_, raz_, = x, y, z, raz,
			
			if abs(ox) == abs(oy):
				x+=X_korak
				y+=Y_korak
			if abs(oy) > abs(ox):
				y+=Y_korak
				if abs(y) >= raz:
					x+=X_korak
					raz+=raz_korak
			if abs(oy) < abs(ox):
				x+=X_korak
				if  abs(x) >= raz:
					y+=Y_korak
					raz+=raz_korak

			if abs(x)>abs(ox):
				x=ox
			if abs(y)>abs(oy):
				y=oy
			
			try:
				self.move(x, y, z, [12, 12, 10, 12, 12, 12,])
			except:
				print 'puko sa:', x_, y_, z_
				x, y, z, raz, = x_, y_, z_- 0.2, raz_,
				if z < 0.0:
					print 'probio pod !'
					raise
			
			print 'x:%s y:%s z:%s' % (x, y, z, )
			time.sleep(0.01)
			if (ox == x) and (oy == y): break
		
		return x, y, z,
	
	def calculate_joints(self, x, y, z):
		""" Place arm gripper in specified point in space, relative to base of arm
			returns joint angles
			
			max_angle - used for ZBUDZ
		"""
		ret = []
		pi, S1, S2, S3, C, H, = 3.141592653, 6.0, 15.5, 12.3, 7.0, 14.0
		if (x == 0) and (y == 0) and (z == 0): return

		z += H
		Teta = (math.asin( y / math.sqrt( pow(x, 2) + pow(y, 2) ))) * 180.0/pi
		la = math.sqrt( pow(x, 2) + pow(y, 2)) + 2
		l = la - C
		za = z - S1
		r = math.sqrt( pow(l, 2) + pow(za, 2) )

		Alfa = (math.acos( (pow(r,2) - pow(S2,2) - pow(S3,2)) / (-2*S2*S3) )) * 180/pi
		Gama1 = (math.acos( (pow(S3,2) - pow(S2,2) - pow(r,2)) / (-2*S2*r) )) * 180/pi
		Gama2 = math.atan(za/l) * 180/pi
		Gama = Gama1 + Gama2
		Beta = Gama + Alfa

		ret.append( 778 - (Teta*5.33333) )
		ret.append( 350 + (Gama*4.444) ) 
		ret.append( 290 + ((Alfa)*5.33333) )
		ret.append( 950 - ((Beta-90)*5.3333) )
		ret.append( None )
		ret.append( None )
		return ret

	OPERATION_SLEEP_REDUCTION = 0.5
	def execute_operation(self, name, speeds=[10,10,10,10,7,7,]):
		if name in self.operations:
			op = self.operations[name]
			iterator_ = op.steps_iterator(self._load)
			if iterator_:
				#print iterator_
				for step in iterator_:
					self.set_raw_position(step.joints, speeds)
					time.sleep(step.interval * self.OPERATION_SLEEP_REDUCTION)
				#self._load += op.load_increment
			else:
				print 'no steps for %s' % (name, )
			
			return None
		else:
			print ' operation %s is not defined' % (name, )






if __name__ == '__main__':
	arm = Arm()
	time.sleep(2.0)

	while 1:
		x = int(raw_input("daj x: "))
		y = int(raw_input("daj y: "))
		z = int(raw_input("daj z: "))
		arm.move(float(x), float(y), float(z))
		#arm._move_smootly(float(x), float(y), float(z))
		print 'sent ..'














